#include "drv_algo.h"

/**
 * @brief:    Algo_PID_Init
 * @param:    pPID - PID结构体指针
 * @param:    Kp - 比例系数
 * @param:    Ki - 积分系数
 * @param:    Kd - 微分系数
 * @param:    max_iout - 积分输出限幅（绝对值）
 * @param:    max_out - 总输出限幅（绝对值）
 */
void Algo_PID_Init(algo_pid_t* pPID, float Kp, float Ki, float Kd, float max_iout, float max_out) 
{
    pPID->Kp = Kp;
    pPID->Ki = Ki;
    pPID->Kd = Kd;
    pPID->error_int = 0;
    pPID->last_error = 0;
    pPID->max_iout = max_iout;
    pPID->max_out = max_out;
}


/**
 * @brief:    Algo_PID
 * @param:    pPID - PID结构体指针
 * @param:    actual - 实际值
 * @param:    target - 目标值
 * @retval:   PID输出值
 */
float Algo_PID_Calc(algo_pid_t* pPID, float actual, float target) {
  float error = target - actual;
  
  // 积分项计算
  pPID->error_int += error;
  
  // 双向积分限幅
  if (pPID->max_iout > 0) {
    if (pPID->error_int > pPID->max_iout) {
      pPID->error_int = pPID->max_iout;
    } else if (pPID->error_int < -pPID->max_iout) {
      pPID->error_int = -pPID->max_iout;
    }
  }
  
  // PID计算
  float out = pPID->Kp * error + 
              pPID->Ki * pPID->error_int + 
              pPID->Kd * (error - pPID->last_error);
  
  // 双向输出限幅
  if (pPID->max_out > 0) {
    if (out > pPID->max_out) {
      out = pPID->max_out;
      // 抗积分饱和：当输出饱和且误差方向一致时，停止积分
      if (error > 0) {
        pPID->error_int -= error;
      }
    } else if (out < -pPID->max_out) {
      out = -pPID->max_out;
      // 抗积分饱和
      if (error < 0) {
        pPID->error_int -= error;
      }
    }
  }
  
  pPID->last_error = error;
  return out;
}


 
pid_struct_t gimbal_yaw_speed_pid;
pid_struct_t gimbal_yaw_angle_pid;
 
void pid_init(pid_struct_t *pid,
              float kp,
              float ki,
              float kd,
              float i_max,
              float out_max)//PID初始化函数
{
  pid->kp      = kp;
  pid->ki      = ki;
  pid->kd      = kd;
  pid->i_max   = i_max;
  pid->out_max = out_max;
}

#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
float pid_calc(pid_struct_t *pid, float ref, float fdb)//PID运算函数
{
  pid->ref = ref;
  pid->fdb = fdb;
  pid->err[1] = pid->err[0];
  pid->err[0] = pid->ref - pid->fdb;
  
  pid->p_out  = pid->kp * pid->err[0];
  pid->i_out += pid->ki * pid->err[0];
  pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
  
  pid->output = pid->p_out + pid->i_out + pid->d_out;
  LIMIT_MIN_MAX(pid->output, -pid->out_max, pid->out_max);
  return pid->output;
}
 
void gimbal_PID_init()//角度环和速度环的PID初始化,只是初测出来的数据，具体还需要测试
{
	pid_init(&gimbal_yaw_speed_pid, 30, 0, 0, 30000, 30000);//P=30,I=0,D=0
	pid_init(&gimbal_yaw_angle_pid, 400, 0, 0, 0, 320);//P=400,I=0,D=0
}











